#include "task/task_process.h"

using namespace task;

TaskProcess::TaskProcess(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
    : nh_(nh),
      private_nh_(private_nh)
{
    Init();
}

TaskProcess::~TaskProcess()
{
    common_lib::SpdLog::Instance()->EndLog();
}

void TaskProcess::Init()
{
    // private_nh_.getParam("param_file", vehicle_param_file_);
    // private_nh_.getParam("path", file_path_);
    // common_lib::SpdLog::Instance()->InitSpdLog(file_path_ + "/log", "task");
    // LINFO("================================== Task Start ==================================");
    // local_host_task_times_ = 10;
    // YAML::Node vehicle_params = YAML::LoadFile(vehicle_param_file_);
    // communication_type_       = vehicle_params["communication_type"].as<std::string>();

    // if (communication_type_ == "localhost")
    // {
    // }

    // sub_actuator_ = nh_.subscribe("/actuator", 10, &TaskProcess::ActuatorCallback, this);
    // sub_control_ = nh_.subscribe("/control", 10, &TaskProcess::ControlCallback, this);
    // sub_control_state_ = nh_.subscribe("/control_state", 10, &TaskProcess::ControlStateCallback, this);

    // pub_task_ = nh_.advertise<task_msgs::task_msg>("/task", 10);

    // srv_client_path_info_ = nh_.serviceClient<map_operation_msgs::GetPathInfo>("/GetPathInfoSrv");
    // srv_client_point_info_ = nh_.serviceClient<map_operation_msgs::GetPointInfo>("/GetPointInfoSrv");

    // ptr_cmd_process_ = std::make_shared<task::CommandProcess>(this);
    // ptr_cmd_process_->Init(vehicle_param_file_);
}

void TaskProcess::TaskInteract(sTaskMsg &path_info)
{
    
}

void TaskProcess::SetMapFile(std::string &map_file)
{
    // Call Map Operation Service
}

void TaskProcess::GetFirmwareInfo(std::vector<sFirmwareInfo> &firminfo)
{
    // Call All Node Firmware Info Service
    firminfo.clear();
}

// void TaskProcess::ControlCallback(const control_msgs::control_msgConstPtr &msg)
// {
//     amr_state_.control = *msg;
// }

// void TaskProcess::ControlStateCallback(const control_msgs::control_state_msgConstPtr &msg)
// {
//     amr_state_.control_state = *msg;
// }

// void TaskProcess::ActuatorCallback(const actuator_msgs::actuator_msgConstPtr &msg)
// {
//     amr_state_.actuator = *msg;
// }

// bool TaskProcess::GetPathInofSrv(map_operation_msgs::GetPathInfoRequest &req, map_operation_msgs::GetPathInfoResponse &resp)
// {
//     if (!srv_client_path_info_.exists())
//         return false;

//     if (srv_client_path_info_.waitForExistence(ros::Duration(0.5f)))
//     {
//         int cnt = 0;
//         while (!srv_client_path_info_.call(req, resp) && (cnt < 5))
//         {
//             cnt++;
//             if (cnt == 5)
//             {
//                 throw("error");
//             }
//             sleep(1);
//         }
//     }
//     return resp.is_valid;
// }

// bool TaskProcess::GetPathInofSrv(map_operation_msgs::GetPathInfo &srv)
// {
//     if (!srv_client_path_info_.exists())
//         return false;
//     if (srv_client_path_info_.waitForExistence(ros::Duration(0.5f)))
//     {
//         int cnt = 0;
//         // printf("TaskProcess::GetPathInofSrv() cnt: %d\n", cnt);
//         while (!srv_client_path_info_.call(srv) && (cnt < 5))
//         {
//             cnt++;
//             if (cnt == 5)
//             {
//                 throw("error");
//                 return false;
//             }
//             sleep(1);
//         }
//     }
//     return srv.response.is_valid;
// }

// bool TaskProcess::GetPointInfoSrv(map_operation_msgs::GetPointInfoRequest &req, map_operation_msgs::GetPointInfoResponse &resp)
// {
//     if (!srv_client_point_info_.exists())
//         return false;
//     if (srv_client_point_info_.waitForExistence(ros::Duration(0.5f)))
//     {
//         int cnt = 0;
//         while (!srv_client_point_info_.call(req, resp) && (cnt < 5))
//         {
//             cnt++;
//             if (cnt == 5)
//             {
//                 throw("error");
//             }
//             sleep(1);
//         }
//     }
//     return resp.is_valid;
// }

// bool TaskProcess::GetPointInfoSrv(map_operation_msgs::GetPointInfo &srv)
// {
//     if (!srv_client_point_info_.exists())
//         return false;
//     if (srv_client_point_info_.waitForExistence(ros::Duration(0.5f)))
//     {
//         int cnt = 0;
//         while (!srv_client_point_info_.call(srv) && (cnt < 5))
//         {
//             cnt++;
//             if (cnt == 5)
//             {
//                 printf("TaskProcess::GetPointInfoSrv() error cnt %s\n", srv.request.pointid.c_str());
//             }
//             sleep(1);
//         }
//     }
//     return srv.response.is_valid;
// }

// bool TaskProcess::GetPointInfoSrv(std::string &pointid, sTaskPoint &point)
// {
//     map_operation_msgs::GetPointInfoResponse resp;
//     resp.is_valid = false;
//     if (!srv_client_point_info_.exists())
//         return resp.is_valid;

//     map_operation_msgs::GetPointInfoRequest req;
//     req.pointid = pointid;
//     if (!GetPointInfoSrv(req, resp))
//     {
//         resp.is_valid = false;
//     }
//     else
//     {
//         point.x = resp.pose.position.x;
//         point.y = resp.pose.position.y;
//         point.z = resp.pose.position.z;
//         point.theta = tf::getYaw(resp.pose.orientation);
//         point.point_id = pointid;
//     }
//     return resp.is_valid;
// }

